#pragma once

#include "mbt/tracker.hh"

enum InnerPointType {
	GRID_IP,
	KLT_IP
};

class DFTracker : public Tracker {
public:
	//DFTracker(const cv::Matx33f& K, const cv::Matx14f& distCoeffs, std::vector<Object3D*>& objects);
	DFTracker(const cv::Matx33f& K, std::vector<Object3D*>& objects);

	virtual void Track(std::vector<cv::Mat>& imagePyramid, std::vector<Object3D*>& objects, int runs = 1) override;
	virtual void ToggleTracking(cv::Mat& frame, int objectIndex, bool undistortFrame = true) override;
	virtual void EstimatePoses(cv::Mat& frame, bool check_lost) override;

protected:
	void ComputeJac(std::vector<cv::Mat>& image_fields, std::vector<cv::Mat>& template_fields,  cv::Matx66f& wJTJM, cv::Matx61f& JTM);
	void RunIteration(std::vector<cv::Mat>& image_fields, std::vector<cv::Mat>& template_fields);
	void SampleGridInnerPoints(std::vector<Object3D*>& objects, int grid_step, std::vector<cv::Point3f>& ctr_pts);
	void ComputeGradientFields(const cv::Mat& image, std::vector<cv::Mat>& desc_fields);
	
protected:
	cv::Mat templ;
	std::vector<cv::Mat> template_fields, image_fields;
	std::vector<cv::Mat> dx_fields, dy_fields;

	std::vector<cv::Point> template_points;
	std::vector<cv::Point> warped_points;
	std::vector<cv::Point3f> ctr_pts;

	int grid_step;
	bool is_normalize_fields;
};